// // 测试库：SimpleFOC 2.2.1
// //测试硬件：SimpleFOC_X V1.0驱动板
// //淘宝店铺：YOUNGFOC    B站：LazierWoo     闲鱼店铺：YOUNGFOC  立创开源广场 用户名：LazyWoo
// //程序功能：位置闭环控制，使用电流环闭环。在串口窗口中输入：T+位置，就可以使得电机闭环转动到指定位置。
// //比如让电机转动至距离原点180°的位置，则输入其弧度制：T3.14
// //注意事项：位置闭环控制中，是***绝对位置控制****。输入的目标角度单位是弧度制，弧度制3.14对应180°

// //☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
// //☆☆☆☆☆☆程序默认设置的供电电压为 12V,用其他电压供电请记得修改!!!!!☆☆☆☆☆☆☆☆☆
// //☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆


// #include <SimpleFOC.h>

// //创建电机对象
// BLDCMotor motor = BLDCMotor(7); //极对数
// //创建驱动器对象
// //3PWM模式，定义三个PWM输出口，以及使能口
// BLDCDriver3PWM driver = BLDCDriver3PWM(25, 32, 33,26);


// //创建AS5600磁编码器对象
// MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
// TwoWire I2Cone = TwoWire(0);

// //创建内置电流传感器，两个ADC引脚对应两相的电流采样
// //采样电阻10mΩ，0.01欧姆，INA240A2放大倍数50.
// InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 34, 35);

// /*
//  * 
//  * BLDCDriver3PWM driver = BLDCDriver3PWM(25, 32, 33,26);
//  * InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 34, 35);
//  * 这两句中，25与34，32与35是相互对应的电机相，顺序不要修改
// */


// //命令设置
// float target_angle = 0;
// Commander command = Commander(Serial);
// void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

// // RTOS任务句柄
// TaskHandle_t positionCheckTaskHandle;

// // 检查位置偏移并发送消息的任务
// void positionCheckTask(void *pvParameters) {
//   int last_state = 0; // 记录上一次的状态：1表示正偏移，-1表示负偏移，0表示在范围内

//   while (true) {
//     // 固定目标位置为0
//     float target_angle = 2.55;

//     // 获取当前电机位置
//     float current_angle = motor.shaftAngle();

//     // 计算位置误差
//     float position_error = current_angle - target_angle;

//     int current_state = 0; // 当前状态

//     // 检查位置偏移是否超过10°
//     if (position_error > 10*_PI/180) { // 正向偏移超过10° 
//       current_state = 1;
//     } else if (position_error < -10*_PI/180) { // 负向偏移超过10° 
//       current_state = -1;
//     }

//     // 仅在状态变化时发送串口信息
//     if (current_state != last_state) {
//       if (current_state == 1) {
//         Serial.println(":2 10\n");
//       } else if (current_state == -1) {
//         Serial.println(":2 -10\n");
//       } else {
//         Serial.println(":2 0\n");
//       }
//       last_state = current_state; // 更新状态
//     }

//     vTaskDelay(10); // 每100ms检查一次
//   }
// }

// // 按键中断标志
// volatile bool buttonPressed = false;

// // 按键中断服务函数
// void IRAM_ATTR handleButtonInterrupt() {
//   buttonPressed = digitalRead(16) == LOW; // 根据引脚状态设置标志位
// }

// void setup() 
// {

//   //磁编码器初始化
//   I2Cone.begin(19,18, 400000UL);
//   sensor.init(&I2Cone);
//   //关联电机对象与磁编码器对象
//   motor.linkSensor(&sensor);

//   // 驱动器参数
//   // ☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆供电电压☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆
//   driver.voltage_power_supply = 24.0;  //板子上带有防反接二极管，在使用过程中约有0.3V的压降，可以减去0.3，也可以忽略
//   //驱动器初始化
//   driver.init();
//   //关联电机对象和驱动器对象
//   motor.linkDriver(&driver);

//   // 电流传感器初始化
//   current_sense.init();
//   //关联电机对象和电流传感器对象
//   motor.linkCurrentSense(&current_sense);
  
//   //力矩控制模式选择：FOC电流（foc_current）、DC电流（dc_current）、电压（voltage）
//   motor.torque_controller = TorqueControlType::foc_current; 
//   // 运动控制模式选择：力矩（torque）、速度(velocity)、位置(angle)
//   motor.controller = MotionControlType::angle;

//   //FOC电流环参数，只有在电流闭环时才有用
//   //dc_current模式下只有Current q项PID
//   //foc_current模式下Current q项PID、Current d项PID都起作用

//   //Current q项参数
//    motor.PID_current_q.P = 3.0;
//    motor.PID_current_q.I = 300.0;
//    motor.PID_current_q.D = 0.0;
//    motor.PID_current_q.output_ramp = 1000.0;
//    //motor.PID_current_q.limit = 8.0;  //电流环的输出限制，应该等于限制电压motor.voltage_limit；
//    //低通滤波器 
//    motor.LPF_current_q.Tf = 0.005;

//   //Current d项参数
//   motor.PID_current_d.P = 3.0;
//   motor.PID_current_d.I = 300.0;
//   motor.PID_current_d.D = 0.0;
//   motor.PID_current_d.output_ramp = 1000.0;
//   //motor.PID_current_d.limit = 8.0;  //电流环的输出限制，应该等于限制电压motor.voltage_limit；
//   //低通滤波器 
//   motor.LPF_current_d.Tf = 0.005;

   
//   // 速度环PID参数
//   motor.PID_velocity.P = 0.12;
//   motor.PID_velocity.I = 1.2;
//   motor.PID_velocity.D = 0.001;
//   motor.PID_velocity.output_ramp = 1000.0;
//   //速度环的输出限制:在电流闭环时，此限制应该等于限制电流motor.current_limit；
//   //速度环的输出限制:在使用电压力矩时（电流开环），此限制应该等于限制电压motor.voltage_limit； 
//   //motor.PID_velocity.limit = 0.8; 
//   // 低通滤波器
//   motor.LPF_velocity.Tf = 0.02;
  
//   // 角度环PID参数
//   motor.P_angle.P = 10.0;
//   motor.P_angle.I = 0.0;
//   motor.P_angle.D = 0.0;
//   motor.P_angle.output_ramp = 1000.0;
//   //motor.P_angle.limit = 30.0;    //角度环的输出限制就等于速度限制velocity_limit
//   // 低通滤波器
//   motor.LPF_angle.Tf = 0.01;

  
//   // 设置限制
//   motor.velocity_limit = 30.0; //速度限制，指角度模式下的速度限制
//   //把速度限制设小，比如设置成3.0或者5.0，角度控制模式下，最高速度会变小，
//   motor.voltage_limit = 8.0;  //电机限制电压
//   motor.current_limit = 0.8;  //电机限制电流


//   //串口用于接受指令
//   Serial.begin(115200);
//   motor.useMonitoring(Serial);


//   // 电机初始化
//   motor.init();
//   // 初始化FOC
//   motor.initFOC();
//   //使用字母T作为命令前缀
//   command.add('T', doTarget, "target angle");

//   //初始化成功，等待接收命令
//   Serial.println(F("Motor ready."));
//   Serial.println(F("Set the target angle using serial terminal:"));
//   _delay(1000);

//     // 创建RTOS任务
//     xTaskCreate(
//         positionCheckTask,       // 任务函数
//         "PositionCheckTask",     // 任务名称
//         1024,                    // 堆栈大小
//         NULL,                    // 任务参数
//         1,                       // 优先级
//         &positionCheckTaskHandle // 任务句柄
//       );

//   // 配置 GPIO16 为输入模式并启用中断
//   pinMode(16, INPUT_PULLUP);
//   attachInterrupt(digitalPinToInterrupt(16), handleButtonInterrupt, CHANGE);

// }

// void loop() 
// {
//   //FOC运行
//   motor.loopFOC();
//   motor.move(2.55); // 将目标位置固定为0

//   // 按键处理逻辑
//   static unsigned long lastSendTime = 0;
//   if (buttonPressed) {
//     unsigned long currentTime = millis();
//     if (currentTime - lastSendTime >= 100) { // 每100ms打印一次
//       Serial.println(":3 50");
//       lastSendTime = currentTime;
//     }
//   }

//   command.run();
// }
